/**
 * Copyright (c) 2014 - 2017, Nordic Semiconductor ASA
 * 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 * 
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 * 
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 * 
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 * 
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 */
/**
 * @brief BLE LED Button Service central and client application main file.
 *
 * This example can be a central for up to 8 peripherals.
 * The peripheral is called ble_app_blinky and can be found in the ble_peripheral
 * folder.
 */

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include "nordic_common.h"
#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "nrf_sdh_soc.h"
#include "nrf_pwr_mgmt.h"
#include "nrf_ble_gatt.h"
#include "app_error.h"
#include "app_uart.h"
#include "app_timer.h"
#include "app_util.h"
#include "bsp_btn_ble.h"
#include "ble.h"
#include "ble_gap.h"
#include "ble_hci.h"
#include "ble_advdata.h"
#include "ble_advertising.h"
#include "ble_conn_params.h"
#include "ble_db_discovery.h"
#include "ble_nus_c.h"
#include "ble_conn_state.h"

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "app_scheduler.h"
#include "ble_dfu.h"
#include "nrf_drv_wdt.h"

#define SW_MAIN_VERSION_S                   "13"

#define SCHED_MAX_EVENT_DATA_SIZE           APP_TIMER_SCHED_EVENT_DATA_SIZE            /**< Maximum size of scheduler events. */
#ifdef SVCALL_AS_NORMAL_FUNCTION
#define SCHED_QUEUE_SIZE                    30                                         /**< Maximum number of events in the scheduler queue. More is needed in case of Serialization. */
#else
#define SCHED_QUEUE_SIZE                    20                                         /**< Maximum number of events in the scheduler queue. */
#endif


#define APP_BLE_CONN_CFG_TAG      1                                     /**< A tag that refers to the BLE stack configuration we set with @ref sd_ble_cfg_set. Default tag is @ref APP_BLE_CONN_CFG_TAG. */
#define APP_BLE_OBSERVER_PRIO     3                                     /**< Application's BLE observer priority. You shouldn't need to modify this value. */

#define CENTRAL_SCANNING_LED      BSP_BOARD_LED_0
#define CENTRAL_CONNECTED_LED     BSP_BOARD_LED_1

#define UART_TX_BUF_SIZE        2048                                    /**< UART TX buffer size. */
#define UART_RX_BUF_SIZE        256                                     /**< UART RX buffer size. */

#define NUS_SERVICE_UUID_TYPE   BLE_UUID_TYPE_VENDOR_BEGIN              /**< UUID type for the Nordic UART Service (vendor specific). */

#define SCAN_INTERVAL             0x00A0                                /**< Determines scan interval in units of 0.625 millisecond. */
#define SCAN_WINDOW               0x0050                                /**< Determines scan window in units of 0.625 millisecond. */
#define SCAN_TIMEOUT              0x0000                                /**< Timout when scanning. 0x0000 disables timeout. */

#define MIN_CONNECTION_INTERVAL   MSEC_TO_UNITS(7.5, UNIT_1_25_MS)      /**< Determines minimum connection interval in milliseconds. */
#define MAX_CONNECTION_INTERVAL   MSEC_TO_UNITS(90, UNIT_1_25_MS)       /**< Determines maximum connection interval in milliseconds. */
#define SLAVE_LATENCY             0                                     /**< Determines slave latency in terms of connection events. */
#define SUPERVISION_TIMEOUT       MSEC_TO_UNITS(4000, UNIT_10_MS)       /**< Determines supervision time-out in units of 10 milliseconds. */

#define UUID16_SIZE             2                                       /**< Size of 16 bit UUID */
#define UUID32_SIZE             4                                       /**< Size of 32 bit UUID */
#define UUID128_SIZE            16                                      /**< Size of 128 bit UUID */

#define ECHOBACK_BLE_UART_DATA  0                                       /**< Echo the UART data that is received over the Nordic UART Service back to the sender. */
#define DB_DISCOVERY_USE_ARRAY  0
#define TARGET_PERIPH_QFB_ONLY  1
#define ALLOW_SCAN_INIT         (false)

#ifdef DEBUG
    #undef  NRF_LOG_DEFAULT_LEVEL
    #define NRF_LOG_DEFAULT_LEVEL NRF_LOG_LEVEL_DEBUG
#endif // DEBUG

/**@brief   Macro for defining multiple ble_nus_c instances.
 *
 * @param   _name   Name of the array of instances.
 * @param   _cnt    Number of instances to define.
 */
#define BLE_NUS_C_ARRAY_DEF(_name, _cnt)                                                            \
static ble_nus_c_t _name[_cnt];                                                                     \
NRF_SDH_BLE_OBSERVERS(_name ## _obs,                                                                \
                     BLE_NUS_C_BLE_OBSERVER_PRIO,                                                   \
                     ble_nus_c_on_ble_evt, &_name, _cnt)

NRF_BLE_GATT_DEF(m_gatt);                                               /**< GATT module instance. */
BLE_NUS_C_ARRAY_DEF(m_nus_c, NRF_SDH_BLE_CENTRAL_LINK_COUNT);           /**< BLE NUS service client instance. */
#if (DB_DISCOVERY_USE_ARRAY>0)
BLE_DB_DISCOVERY_ARRAY_DEF(m_db_disc, NRF_SDH_BLE_CENTRAL_LINK_COUNT);  /**< Database discovery module instances. */
#else
BLE_DB_DISCOVERY_DEF(m_db_disc);                                        /**< Database discovery module instances. */
#endif

#if (TARGET_PERIPH_QFB_ONLY>0)
static char const m_target_periph_name[] = "QFB";//;                    /**< Name of the device we try to connect to. This name is searched for in the scan report data*/
#else
static char const m_target_periph_name[] = "PlaymateAir";//;            /**< Name of the device we try to connect to. This name is searched for in the scan report data*/
#endif

static bool m_bool_allow_scan;
static bool m_bool_is_scanning;

/**@brief Scan parameters requested for scanning and connection. */
static ble_gap_scan_params_t const m_scan_params =
{
    .active   = 0,
    .interval = SCAN_INTERVAL,
    .window   = SCAN_WINDOW,
    .timeout  = SCAN_TIMEOUT,
    #if (NRF_SD_BLE_API_VERSION <= 2)
        .selective   = 0,
        .p_whitelist = NULL,
    #endif
    #if (NRF_SD_BLE_API_VERSION >= 3)
        .use_whitelist  = 0,
        .adv_dir_report = 0,
    #endif
};

/**@brief Connection parameters requested for connection. */
static ble_gap_conn_params_t const m_connection_param =
{
    (uint16_t)MIN_CONNECTION_INTERVAL,
    (uint16_t)MAX_CONNECTION_INTERVAL,
    (uint16_t)SLAVE_LATENCY,
    (uint16_t)SUPERVISION_TIMEOUT
};


/**@brief NUS uuid. */
static ble_uuid_t const m_nus_uuid =
{
    .uuid = BLE_UUID_NUS_SERVICE,
    .type = NUS_SERVICE_UUID_TYPE
};


/**@brief Function to handle asserts in the SoftDevice.
 *
 * @details This function will be called in case of an assert in the SoftDevice.
 *
 * @warning This handler is an example only and does not fit a final product. You need to analyze
 *          how your product is supposed to react in case of Assert.
 * @warning On assert from the SoftDevice, the system can only recover on reset.
 *
 * @param[in] line_num     Line number of the failing ASSERT call.
 * @param[in] p_file_name  File name of the failing ASSERT call.
 */
void assert_nrf_callback(uint16_t line_num, const uint8_t * p_file_name)
{
    app_error_handler(0xDEADBEEF, line_num, p_file_name);
}


/**
 * @brief Parses advertisement data, providing length and location of the field in case
 *        matching data is found.
 *
 * @param[in]  type       Type of data to be looked for in advertisement data.
 * @param[in]  p_advdata  Advertisement report length and pointer to report.
 * @param[out] p_typedata If data type requested is found in the data report, type data length and
 *                        pointer to data will be populated here.
 *
 * @retval NRF_SUCCESS if the data type is found in the report.
 * @retval NRF_ERROR_NOT_FOUND if the data type could not be found.
 */
static uint32_t adv_report_parse(uint8_t type, uint8_array_t * p_advdata, uint8_array_t * p_typedata)
{
    uint32_t  index = 0;
    uint8_t * p_data;

    p_data = p_advdata->p_data;

    while (index < p_advdata->size)
    {
        uint8_t field_length = p_data[index];
        uint8_t field_type   = p_data[index + 1];

        if (field_type == type)
        {
            p_typedata->p_data = &p_data[index + 2];
            p_typedata->size   = field_length - 1;
            return NRF_SUCCESS;
        }
        index += field_length + 1;
    }
    return NRF_ERROR_NOT_FOUND;
}


/**@brief Function to restart scanning. */
APP_TIMER_DEF(m_restart_scan_timer_id);
static void restart_scan_timer_handler(void * p_context)
{
    ret_code_t ret;

    if (m_bool_is_scanning == false)
    {
		bool discovery_isbusy;
		#if (DB_DISCOVERY_USE_ARRAY>0)
		discovery_isbusy = false;
		for (ret=0; ret<NRF_SDH_BLE_CENTRAL_LINK_COUNT; ret++)
		{
			if(m_db_disc[ret].discovery_in_progress)
			{
				discovery_isbusy = true; break;
			}
		}
		#else
		discovery_isbusy = m_db_disc.discovery_in_progress;
		#endif
        if (discovery_isbusy) // NRF_ERROR_BUSY
        {
            ret = app_timer_start(m_restart_scan_timer_id, 
                APP_TIMER_TICKS(100), (void*)NULL);
            APP_ERROR_CHECK(ret);
            NRF_LOG_DEBUG("restart_scan_timer.");
            return ;
        }
    
        NRF_LOG_INFO("* Start scanning for device name %s.", (uint32_t)m_target_periph_name);
        ret = sd_ble_gap_scan_start(&m_scan_params);
        APP_ERROR_CHECK(ret);
        m_bool_is_scanning = true;
    }
    
    if (ble_conn_state_n_centrals() == 0)
    {
        ret = bsp_indication_set(BSP_INDICATE_SCANNING);
        APP_ERROR_CHECK(ret);        
    }
}
static void timer_restart_scan_init(void)
{
    ret_code_t err_code = app_timer_create(&m_restart_scan_timer_id,
                                        APP_TIMER_MODE_SINGLE_SHOT,
                                        restart_scan_timer_handler);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function to start scanning. */
static void scan_start(void)
{
    ret_code_t ret;

    if (m_bool_allow_scan)
    {
        ret = app_timer_start(m_restart_scan_timer_id, 
            APP_TIMER_TICKS(500), (void*)NULL);
        APP_ERROR_CHECK(ret);
    }
    else
    {
        (void)app_timer_stop(m_restart_scan_timer_id);
        if (m_bool_is_scanning == true)
        {
            (void) sd_ble_gap_scan_stop();
            m_bool_is_scanning = false;
        }
    }
}


// for send data to the peripheral
#if (ECHOBACK_BLE_UART_DATA)
static uint32_t nus_c_string_sendout(uint16_t conn_handle, uint8_t * p_string, uint16_t length)
{
    uint32_t ret_val = NRF_ERROR_INVALID_STATE;
    if(m_nus_c[conn_handle].conn_handle != BLE_CONN_HANDLE_INVALID) 
    {
        do
        {
            ret_val = ble_nus_c_string_send(&m_nus_c[conn_handle], p_string, length);
            if ( (ret_val != NRF_ERROR_INVALID_STATE) && (ret_val != NRF_ERROR_BUSY) )
            {
                APP_ERROR_CHECK(ret_val);
            }
        } while (ret_val == NRF_ERROR_BUSY);
    }
    return ret_val;
}
#endif

static ret_code_t nus_c_char_uart_print(uint8_t byte)
{
    ret_code_t ret_val;
    do
    {
        ret_val = app_uart_put(byte);
        if ((ret_val != NRF_SUCCESS) && (ret_val != NRF_ERROR_BUSY))
        {
            NRF_LOG_ERROR("app_uart_put failed. (0x%04X)", ret_val);
            APP_ERROR_CHECK(ret_val);
        }
    } while (ret_val == NRF_ERROR_BUSY);
    return ret_val;
}

/**@brief Function for handling characters received by the Nordic UART Service.
 *
 * @details This function takes a list of characters of length data_len and prints the characters out on UART.
 *          If @ref ECHOBACK_BLE_UART_DATA is set, the data is sent back to sender.
 */
static void ble_nus_chars_received_uart_print(uint16_t conn_handle, uint8_t * p_data, uint16_t data_len)
{
    uint32_t i;
    uint8_t szPrefix[MAX((BLE_GAP_ADDR_LEN*3+10), 24)];

    NRF_LOG_DEBUG("Receiving data.");
    NRF_LOG_HEXDUMP_DEBUG(p_data, data_len);

    sprintf((char *)szPrefix, "[%d:%02X-%02X-%02X-%02X-%02X-%02X]", 
        (uint8_t)conn_handle,
        m_nus_c[conn_handle].peer_addr.addr[0],
        m_nus_c[conn_handle].peer_addr.addr[1],
        m_nus_c[conn_handle].peer_addr.addr[2],
        m_nus_c[conn_handle].peer_addr.addr[3],
        m_nus_c[conn_handle].peer_addr.addr[4],
        m_nus_c[conn_handle].peer_addr.addr[5] );
    
    for (i = 0; szPrefix[i]; i++)
    {
        nus_c_char_uart_print(szPrefix[i]);
    }
    
    for (i = 0; i < data_len; i++)
    {
        nus_c_char_uart_print(p_data[i]);
    }
    
    #if 1
    nus_c_char_uart_print('\r');
    nus_c_char_uart_print('\n');
    #endif
    #if (ECHOBACK_BLE_UART_DATA)
    {
        // Send data back to peripheral.
        nus_c_string_sendout(conn_handle, p_data, data_len);
    }
    #endif
}


/**@brief   Function for handling app_uart events.
 *
 * @details This function will receive a single character from the app_uart module and append it to
 *          a string. The string will be be sent over BLE when the last character received was a
 *          'new line' '\n' (hex 0x0A) or if the string has reached the maximum data length.
 */
void uart_event_handle(app_uart_evt_t * p_event)
{

    switch (p_event->evt_type)
    {
        /**@snippet [Handling data from UART] */
        case APP_UART_DATA_READY:
            {
                extern void user_task_command_recv(void * p_event_data, uint16_t event_size);
                static volatile uint8_t u8event_cnt = 0;
                if (u8event_cnt<1)
                {
                    uint8_t * pu8event_cnt = (void *)&u8event_cnt;
                    uint32_t err_code = app_sched_event_put((void *)&pu8event_cnt, sizeof(pu8event_cnt), user_task_command_recv);
                    if (err_code == NRF_SUCCESS) u8event_cnt++;
                }
            }
            break;

        /**@snippet [Handling data from UART] */
        case APP_UART_COMMUNICATION_ERROR:
            NRF_LOG_ERROR("Communication error occurred while handling UART.");
            APP_ERROR_HANDLER(p_event->data.error_communication);
            break;

        case APP_UART_FIFO_ERROR:
            NRF_LOG_ERROR("Error occurred in FIFO module used by UART.");
            APP_ERROR_HANDLER(p_event->data.error_code);
            break;

        default:
            break;
    }
}


/**@brief Callback handling NUS Client events.
 *
 * @details This function is called to notify the application of NUS client events.
 *
 * @param[in]   p_ble_nus_c   NUS Client Handle. This identifies the NUS client
 * @param[in]   p_ble_nus_evt Pointer to the NUS Client event.
 */

/**@snippet [Handling events from the ble_nus_c module] */
static void ble_nus_c_evt_handler(ble_nus_c_t * p_ble_nus_c, ble_nus_c_evt_t const * p_ble_nus_evt)
{
    ret_code_t err_code;
    
    switch (p_ble_nus_evt->evt_type)
    {
        case BLE_NUS_C_EVT_DISCOVERY_COMPLETE:
            NRF_LOG_INFO("NUS service discovered on conn_handle 0x%x",
                         p_ble_nus_c->conn_handle);

            err_code = ble_nus_c_handles_assign(p_ble_nus_c, p_ble_nus_evt->conn_handle, &p_ble_nus_evt->handles);
            APP_ERROR_CHECK(err_code);

            err_code = ble_nus_c_tx_notif_enable(p_ble_nus_c);
            APP_ERROR_CHECK(err_code);
            break; // BLE_NUS_C_EVT_DISCOVERY_COMPLETE

        case BLE_NUS_C_EVT_NUS_TX_EVT:
            ble_nus_chars_received_uart_print(p_ble_nus_c->conn_handle, 
                p_ble_nus_evt->p_data, p_ble_nus_evt->data_len);
            break;

        case BLE_NUS_C_EVT_DISCONNECTED:
            //NRF_LOG_DEBUG("Link 0x%x Disconnected.", p_ble_nus_c->conn_handle);
            //scan_start();
            break;
        default:
            // No implementation needed.
            break;
    }
}
/**@snippet [Handling events from the ble_nus_c module] */


/**
 * @brief Function for shutdown events.
 *
 * @param[in]   event       Shutdown type.
 */
static bool shutdown_handler(nrf_pwr_mgmt_evt_t event)
{
    ret_code_t err_code;

    err_code = bsp_indication_set(BSP_INDICATE_IDLE);
    APP_ERROR_CHECK(err_code);

    switch (event)
    {
        case NRF_PWR_MGMT_EVT_PREPARE_WAKEUP:
            // Prepare wakeup buttons.
            err_code = bsp_btn_ble_sleep_mode_prepare();
            APP_ERROR_CHECK(err_code);
            break;

        default:
            break;
    }

    return true;
}

NRF_PWR_MGMT_HANDLER_REGISTER(shutdown_handler, APP_SHUTDOWN_HANDLER_PRIORITY);

/**@brief Reads an advertising report and checks if a UUID is present in the service list.
 *
 * @details The function is able to search for 16-bit, 32-bit and 128-bit service UUIDs.
 *          To see the format of a advertisement packet, see
 *          https://www.bluetooth.org/Technical/AssignedNumbers/generic_access_profile.htm
 *
 * @param[in]   p_target_uuid The UUID to search for.
 * @param[in]   p_adv_report  Pointer to the advertisement report.
 *
 * @retval      true if the UUID is present in the advertisement report. Otherwise false
 */
static bool is_uuid_present(ble_uuid_t               const * p_target_uuid,
                            ble_gap_evt_adv_report_t const * p_adv_report)
{
    ret_code_t   err_code;
    ble_uuid_t   extracted_uuid;
    uint16_t     index  = 0;
    uint8_t    * p_data = (uint8_t *)p_adv_report->data;

    NRF_LOG_HEXDUMP_INFO(p_data, p_adv_report->dlen);
    while (index < p_adv_report->dlen)
    {
        uint8_t field_length = p_data[index];
        uint8_t field_type   = p_data[index + 1];

        if (   (field_type == BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE)
            || (field_type == BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE))
        {
            for (uint32_t i = 0; i < (field_length / UUID16_SIZE); i++)
            {
                err_code = sd_ble_uuid_decode(UUID16_SIZE,
                                              &p_data[i * UUID16_SIZE + index + 2],
                                              &extracted_uuid);

                if (err_code == NRF_SUCCESS)
                {
                    if (extracted_uuid.uuid == p_target_uuid->uuid)
                    {
                        return true;
                    }
                }
            }
        }
        else if (   (field_type == BLE_GAP_AD_TYPE_32BIT_SERVICE_UUID_MORE_AVAILABLE)
                 || (field_type == BLE_GAP_AD_TYPE_32BIT_SERVICE_UUID_COMPLETE))
        {
            for (uint32_t i = 0; i < (field_length / UUID32_SIZE); i++)
            {
                err_code = sd_ble_uuid_decode(UUID32_SIZE,
                                              &p_data[i * UUID32_SIZE + index + 2],
                                              &extracted_uuid);

                if (err_code == NRF_SUCCESS)
                {
                    if (   (extracted_uuid.uuid == p_target_uuid->uuid)
                        && (extracted_uuid.type == p_target_uuid->type))
                    {
                        return true;
                    }
                }
            }
        }

        else if (   (field_type == BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_MORE_AVAILABLE)
                 || (field_type == BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE))
        {
            err_code = sd_ble_uuid_decode(UUID128_SIZE, &p_data[index + 2], &extracted_uuid);
            if (err_code == NRF_SUCCESS)
            {
                if (   (extracted_uuid.uuid == p_target_uuid->uuid)
                    && (extracted_uuid.type == p_target_uuid->type))
                {
                    return true;
                }
            }
        }
        index += field_length + 1;
    }

    #if (TARGET_PERIPH_QFB_ONLY>0) // for QFB only
    if (p_adv_report->dlen <= 8)
    {
        index = 0;
        while (index < p_adv_report->dlen)
        {
            uint8_t field_length = p_data[index];
            uint8_t field_type   = p_data[index + 1];
            if (  (field_type == BLE_GAP_AD_TYPE_FLAGS)
                &&(field_length == (AD_TYPE_FLAGS_DATA_SIZE+AD_TYPE_FIELD_SIZE)) )
            {
                if (p_data[index + 2]==0x06)
                {
                    return true;
                }
            }
            index += field_length + 1;
        }
    }
    #endif
    return false;
}


/**@brief Function for handling the advertising report BLE event.
 *
 * @param[in] p_ble_evt  Bluetooth stack event.
 */
static void on_adv_report(ble_evt_t const * p_ble_evt)
{
    uint32_t      err_code;
    uint8_array_t adv_data;
    uint8_array_t dev_name;
    bool          do_connect = false;

    // For readibility.
    ble_gap_evt_t  const * p_gap_evt  = &p_ble_evt->evt.gap_evt;
    ble_gap_addr_t const * peer_addr  = &p_gap_evt->params.adv_report.peer_addr;

    // Prepare advertisement report for parsing.
    adv_data.p_data = (uint8_t *)p_gap_evt->params.adv_report.data;
    adv_data.size   = p_gap_evt->params.adv_report.dlen;

    // Search for advertising names.
    bool found_name = false;
    err_code = adv_report_parse(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
                                &adv_data,
                                &dev_name);
    if (err_code != NRF_SUCCESS)
    {
        // Look for the short local name if it was not found as complete.
        err_code = adv_report_parse(BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME, &adv_data, &dev_name);
        if (err_code != NRF_SUCCESS)
        {
            // If we can't parse the data, then exit.
            return;
        }
        else
        {
            found_name = true;
        }
    }
    else
    {
        found_name = true;
    }

    if (found_name)
    {
        if (strlen(m_target_periph_name) != 0)
        {
            if (memcmp(m_target_periph_name, dev_name.p_data, dev_name.size) == 0)
            {
                do_connect = true;
            }
        }
    }

    if (do_connect)
    {
        ble_gap_evt_adv_report_t const * p_adv_report = &p_gap_evt->params.adv_report;
        do_connect = is_uuid_present(&m_nus_uuid, p_adv_report);
        if (!do_connect)
        {
            NRF_LOG_INFO("is_uuid_present = false");
        }
    }

    if (do_connect)
    {
        for (uint32_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
        {
            if(m_nus_c[i].conn_handle == BLE_CONN_HANDLE_INVALID) continue;
            if (memcmp(peer_addr->addr, m_nus_c[i].peer_addr.addr, sizeof(peer_addr->addr)) == 0)
            {
                do_connect = false;
                break;
            }
        }
    }

    if (do_connect)
    {
        // Initiate connection.
        err_code = sd_ble_gap_connect(peer_addr, &m_scan_params, &m_connection_param, APP_BLE_CONN_CFG_TAG);
        if (err_code != NRF_SUCCESS)
        {
            NRF_LOG_ERROR("Connection Request Failed, reason %d", err_code);
        }
        else
        {
            //err_code = bsp_indication_set(BSP_INDICATE_IDLE);
            //APP_ERROR_CHECK(err_code);
            
            NRF_LOG_INFO("Connecting to target %02X-%02X-%02X-%02X-%02X-%02X",
                     peer_addr->addr[0],
                     peer_addr->addr[1],
                     peer_addr->addr[2],
                     peer_addr->addr[3],
                     peer_addr->addr[4],
                     peer_addr->addr[5]
                     );
        }
        // scan is automatically stopped by the connect
        m_bool_is_scanning = false;
    }
}


/**@brief Function for handling BLE events.
 *
 * @param[in]   p_ble_evt   Bluetooth stack event.
 * @param[in]   p_context   Unused.
 */
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
{
    ret_code_t err_code;

    // For readability.
    ble_gap_evt_t const * p_gap_evt = &p_ble_evt->evt.gap_evt;

    switch (p_ble_evt->header.evt_id)
    {
        // Upon connection, check which peripheral has connected, initiate DB
        // discovery, update LEDs status and resume scanning if necessary.
        case BLE_GAP_EVT_CONNECTED:
        {
            NRF_LOG_INFO("Connection 0x%x established, starting DB discovery.",
                         p_gap_evt->conn_handle);

            APP_ERROR_CHECK_BOOL(p_gap_evt->conn_handle < NRF_SDH_BLE_CENTRAL_LINK_COUNT);

            memcpy(&m_nus_c[p_gap_evt->conn_handle].peer_addr,
                &p_gap_evt->params.connected.peer_addr, sizeof(ble_gap_addr_t));

            err_code = ble_nus_c_handles_assign(&m_nus_c[p_gap_evt->conn_handle],
                                                p_gap_evt->conn_handle,
                                                NULL);
            APP_ERROR_CHECK(err_code);

            err_code = ble_db_discovery_start(
                                              #if (DB_DISCOVERY_USE_ARRAY>0)
                                              &m_db_disc[p_gap_evt->conn_handle],
                                              #else
                                              &m_db_disc,
                                              #endif
                                              p_gap_evt->conn_handle);
            //if (err_code != NRF_ERROR_BUSY)
            {
                APP_ERROR_CHECK(err_code);
            }

            // Update LEDs status, and check if we should be looking for more
            // peripherals to connect to.
            //bsp_board_led_on(CENTRAL_CONNECTED_LED);
            err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
            APP_ERROR_CHECK(err_code);

            if (ble_conn_state_n_centrals() == NRF_SDH_BLE_CENTRAL_LINK_COUNT)
            {
                //bsp_board_led_off(CENTRAL_SCANNING_LED);
            }
            else
            {
                // Resume scanning.
                //bsp_board_led_on(CENTRAL_SCANNING_LED);
                scan_start();
            }
        } break; // BLE_GAP_EVT_CONNECTED

        // Upon disconnection, reset the connection handle of the peer which disconnected, update
        // the LEDs status and start scanning again.
        case BLE_GAP_EVT_DISCONNECTED:
        {
            NRF_LOG_INFO("NUS central link 0x%x disconnected (reason: 0x%x)",
                         p_gap_evt->conn_handle,
                         p_gap_evt->params.disconnected.reason);

            APP_ERROR_CHECK_BOOL(p_gap_evt->conn_handle < NRF_SDH_BLE_CENTRAL_LINK_COUNT);

            memset(&m_nus_c[p_gap_evt->conn_handle].peer_addr,
                0, sizeof(ble_gap_addr_t));
            
            //if (ble_conn_state_n_centrals() == 0)
            {
                // Turn off connection indication LED
                //bsp_board_led_off(CENTRAL_CONNECTED_LED);
            }

            if (m_bool_is_scanning == false)
            {
                // Resume scanning.
                scan_start();
            }

            // Turn on LED for indicating scanning
            //bsp_board_led_on(CENTRAL_SCANNING_LED);
        }    break;

        case BLE_GAP_EVT_ADV_REPORT:
            on_adv_report(p_ble_evt);
            break;

        case BLE_GAP_EVT_TIMEOUT:
            NRF_LOG_DEBUG("BLE_GAP_EVT_TIMEOUT.");
            // We have not specified a timeout for scanning, so only connection attemps can timeout.
            if (p_gap_evt->params.timeout.src == BLE_GAP_TIMEOUT_SRC_SCAN)
            {
                NRF_LOG_INFO("Scan timed out.");
                m_bool_is_scanning = false;
                scan_start();
            }
            else if (p_gap_evt->params.timeout.src == BLE_GAP_TIMEOUT_SRC_CONN)
            {
                NRF_LOG_INFO("Connection Request timed out.");
            }
            break;

        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
            NRF_LOG_DEBUG("BLE_GAP_EVT_SEC_PARAMS_REQUEST.");
            // Pairing not supported
            err_code = sd_ble_gap_sec_params_reply(p_ble_evt->evt.gap_evt.conn_handle, BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPP, NULL, NULL);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST.");
            // Accept parameters requested by peer.
            err_code = sd_ble_gap_conn_param_update(p_gap_evt->conn_handle,
                                        &p_gap_evt->params.conn_param_update_request.conn_params);
            APP_ERROR_CHECK(err_code);
        } break;

#ifndef S140
        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("PHY update request.");
            ble_gap_phys_t const phys =
            {
                .rx_phys = BLE_GAP_PHY_AUTO,
                .tx_phys = BLE_GAP_PHY_AUTO,
            };
            err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
            APP_ERROR_CHECK(err_code);
        } break;
#endif

        case BLE_GATTC_EVT_TIMEOUT:
        {
            // Disconnect on GATT Client timeout event.
            NRF_LOG_DEBUG("GATT Client Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
        } break;

        case BLE_GATTS_EVT_TIMEOUT:
        {
            // Disconnect on GATT Server timeout event.
            NRF_LOG_DEBUG("GATT Server Timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
        } break;

        default:
            // No implementation needed.
            break;
    }
}


/**@brief Function for initializing the BLE stack.
 *
 * @details Initializes the SoftDevice and the BLE event interrupts.
 */
static void ble_stack_init(void)
{
    ret_code_t err_code;

    err_code = nrf_sdh_enable_request();
    APP_ERROR_CHECK(err_code);

    // Configure the BLE stack using the default settings.
    // Fetch the start address of the application RAM.
    uint32_t ram_start = 0;
    err_code = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
    APP_ERROR_CHECK(err_code);

    // Enable BLE stack.
    err_code = nrf_sdh_ble_enable(&ram_start);
    APP_ERROR_CHECK(err_code);

    // Register a handler for BLE events.
    NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
}


/**@brief Function for handling events from the GATT library. */
void gatt_evt_handler(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt)
{
    if (p_evt->evt_id == NRF_BLE_GATT_EVT_ATT_MTU_UPDATED)
    {
        uint16_t max_dlen = p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH;
        NRF_LOG_INFO("ATT MTU exchange completed.");

        m_nus_c[p_evt->conn_handle].max_data_len = max_dlen;
        NRF_LOG_INFO("Ble NUS 0x%X max data length set to 0x%X(%d)", p_evt->conn_handle, max_dlen, max_dlen);
    }
}


/**@brief Function for initializing the GATT module.
 */
static void gatt_init(void)
{
    ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_gatt_att_mtu_central_set(&m_gatt, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling events from the BSP module.
 *
 * @param[in] event  Event generated by button press.
 */
void bsp_event_handler(bsp_event_t event)
{
    ret_code_t err_code;

    switch (event)
    {
        case BSP_EVENT_SLEEP:
            nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_SYSOFF);
            break;

        case BSP_EVENT_DISCONNECT:
            NRF_LOG_DEBUG("BSP_EVENT_DISCONNECT.");
            for (uint32_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
            {
                if(m_nus_c[i].conn_handle == BLE_CONN_HANDLE_INVALID) continue;
                err_code = sd_ble_gap_disconnect(m_nus_c[i].conn_handle,
                                                 BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
                if (err_code != NRF_ERROR_INVALID_STATE)
                {
                    APP_ERROR_CHECK(err_code);
                }
            }
            break;

        default:
            break;
    }
}

/**@brief Function for initializing the UART. */
static void uart_init(void)
{
    ret_code_t err_code;

    app_uart_comm_params_t const comm_params =
    {
        #if 1
        .rx_pin_no    = 8,// RX_PIN_NUMBER,//
        .tx_pin_no    = 6,// TX_PIN_NUMBER,//
        .rts_pin_no   = 7,// RTS_PIN_NUMBER,//
        .cts_pin_no   = 5,// CTS_PIN_NUMBER,//
        .flow_control = APP_UART_FLOW_CONTROL_ENABLED,
        #else
        .rx_pin_no    = 31,// RX_PIN_NUMBER,//
        .tx_pin_no    = 30,// TX_PIN_NUMBER,//
        .rts_pin_no   = 29,// RTS_PIN_NUMBER,//
        .cts_pin_no   = 28,// CTS_PIN_NUMBER,//
        .flow_control = APP_UART_FLOW_CONTROL_DISABLED,
        #endif
        .use_parity   = false,
        .baud_rate    = UART_BAUDRATE_BAUDRATE_Baud115200
    };

    APP_UART_FIFO_INIT(&comm_params,
                       UART_RX_BUF_SIZE,
                       UART_TX_BUF_SIZE,
                       uart_event_handle,
                       APP_IRQ_PRIORITY_LOWEST,
                       err_code);

    APP_ERROR_CHECK(err_code);
}


/**@brief LED Button collector initialization. */
static void nus_c_init(void)
{
    ret_code_t       err_code;
    ble_nus_c_init_t nus_c_init_obj;

    nus_c_init_obj.evt_handler = ble_nus_c_evt_handler;

    for (uint32_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
    {
        err_code = ble_nus_c_init(&m_nus_c[i], &nus_c_init_obj);
        APP_ERROR_CHECK(err_code);
    }

    m_bool_allow_scan = ALLOW_SCAN_INIT;
    m_bool_is_scanning = false;
}


/**@brief Function for initializing buttons and leds. */
static void buttons_leds_init(void)
{
    ret_code_t err_code;
    bsp_event_t startup_event;

    err_code = bsp_init(BSP_INIT_LED, bsp_event_handler);
    APP_ERROR_CHECK(err_code);

    err_code = bsp_btn_ble_init(NULL, &startup_event);
    APP_ERROR_CHECK(err_code);

    //bsp_board_leds_init();
}


/** @brief Function for initializing the timer.
 */
static void timer_init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);
    timer_restart_scan_init();
}


/** @brief Function for initializing the log module.
 */
static void log_init(void)
{
    ret_code_t err_code = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(err_code);

    NRF_LOG_DEFAULT_BACKENDS_INIT();
}


/**@brief Function for initializing the Power manager. */
static void power_init(void)
{
    ret_code_t err_code = nrf_pwr_mgmt_init();
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling database discovery events.
 *
 * @details This function is callback function to handle events from the database discovery module.
 *          Depending on the UUIDs that are discovered, this function should forward the events
 *          to their respective services.
 *
 * @param[in] p_event  Pointer to the database discovery event.
 */
static void db_disc_handler(ble_db_discovery_evt_t * p_evt)
{
    NRF_LOG_DEBUG("call to ble_nus_on_db_disc_evt for instance %d and link 0x%x!",
                  p_evt->conn_handle,
                  p_evt->conn_handle);

    ble_nus_c_on_db_disc_evt(&m_nus_c[p_evt->conn_handle], p_evt);
}


/** @brief Database discovery initialization.
 */
static void db_discovery_init(void)
{
    ret_code_t err_code = ble_db_discovery_init(db_disc_handler);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for the Event Scheduler initialization.
 */
static void scheduler_init(void)
{
    #if NRF_MODULE_ENABLED(APP_SCHEDULER)
    APP_SCHED_INIT(SCHED_MAX_EVENT_DATA_SIZE, SCHED_QUEUE_SIZE);
    #endif //NRF_MODULE_ENABLED(APP_SCHEDULER)
}


/** @brief WDT initialization.
 */
#if NRF_MODULE_ENABLED(WDT)
static nrf_drv_wdt_channel_id m_channel_id;
APP_TIMER_DEF(m_feed_wdt_timer_id);
static void feed_wdt_timer_handler(void * p_context)
{
    nrf_drv_wdt_channel_feed(m_channel_id);
}
static void timer_feed_wdt_init(void)
{
    ret_code_t err_code = app_timer_create(&m_feed_wdt_timer_id,
                                        APP_TIMER_MODE_REPEATED,
                                        feed_wdt_timer_handler);
    APP_ERROR_CHECK(err_code);
    
    err_code = app_timer_start(m_feed_wdt_timer_id, 
        APP_TIMER_TICKS(500), (void*)NULL);
    APP_ERROR_CHECK(err_code);
}
/**
 * @brief WDT events handler.
 */
void wdt_event_handler(void)
{
    //NOTE: The max amount of time we can spend in WDT interrupt is two cycles of 32768[Hz] clock - after that, reset occurs
}
#endif //NRF_MODULE_ENABLED(WDT)
static void watchdog_init(void)
{
    uint32_t reset_reason = 0;
    sd_power_reset_reason_get(&reset_reason);
    if (reset_reason & POWER_RESETREAS_DOG_Msk) m_bool_allow_scan = true;
    // if is WDT reset then auto scan.
    #if NRF_MODULE_ENABLED(WDT)
    {
        //Configure WDT.
        nrf_drv_wdt_config_t config = NRF_DRV_WDT_DEAFULT_CONFIG;
        uint32_t err_code = nrf_drv_wdt_init(&config, wdt_event_handler);
        APP_ERROR_CHECK(err_code);
        err_code = nrf_drv_wdt_channel_alloc(&m_channel_id);
        APP_ERROR_CHECK(err_code);
        nrf_drv_wdt_enable();
        timer_feed_wdt_init();
    }
    #endif //NRF_MODULE_ENABLED(WDT)
    sd_power_reset_reason_clr(reset_reason);
    NRF_LOG_INFO("reset_reason = 0x%02X", reset_reason);
}


int main(void)
{
    log_init();
    scheduler_init();
    timer_init();
    power_init();
    uart_init();
    buttons_leds_init();
    ble_stack_init();
    gatt_init();
    db_discovery_init();
    nus_c_init();
    ble_conn_state_init();
    watchdog_init();

    NRF_LOG_INFO("Multilink central started.");NRF_LOG_DEBUG("Debug.");//printf("start.\r\n");

    // Start scanning for peripherals and initiate connection to devices which  advertise.
    scan_start();

    // Turn on the LED to signal scanning.
    //bsp_board_led_on(CENTRAL_SCANNING_LED);

    for (;;)
    {
        #if NRF_MODULE_ENABLED(APP_SCHEDULER)
        app_sched_execute();
        #endif //NRF_MODULE_ENABLED(APP_SCHEDULER)
        if (NRF_LOG_PROCESS() == false)
        {
            nrf_pwr_mgmt_run();
        }
    }
}

//////////////////////////////////////////////////////////////////////////
int JL_strToken2Ary(char *str_ptr, const char *filter_ptr, char *tokenAry[], int aryMax)
{
    char *inner_ptr=NULL;
    int   argc = 0;
    char *outer_ptr = strtok_r(str_ptr, filter_ptr, &inner_ptr);
    while (outer_ptr)
    {
        tokenAry[argc++] = outer_ptr;
        outer_ptr = strtok_r(NULL, filter_ptr, &inner_ptr);
        if(argc>=aryMax) break;
    }
    return argc; // real number of strings.
}

static void host_do_open(void)
{
    if (!m_bool_allow_scan)
    {
        m_bool_allow_scan = true;
        scan_start();
    }
}
static void host_do_close(void)
{
    uint32_t err_code;
    m_bool_allow_scan = false;
    scan_start();
    err_code = bsp_indication_set(BSP_INDICATE_IDLE);
    APP_ERROR_CHECK(err_code);
    
    for (uint32_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
    {
        if(m_nus_c[i].conn_handle == BLE_CONN_HANDLE_INVALID) continue;
        err_code = sd_ble_gap_disconnect(m_nus_c[i].conn_handle,
                                         BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        if (err_code != NRF_ERROR_INVALID_STATE)
        {
            APP_ERROR_CHECK(err_code);
        }
    }
    err_code = err_code;
}
static void host_do_reset(void)
{
    nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_RESET);
}
static void host_do_dfu(void)
{
    uint32_t err_code;
    
    host_do_close();
    
    err_code = sd_power_gpregret_clr(0, 0xffffffff);
    //APP_ERROR_CHECK(err_code);

    err_code = sd_power_gpregret_set(0, BOOTLOADER_DFU_START);
    //APP_ERROR_CHECK(err_code);

    // Signal that DFU mode is to be enter to the power management module
    nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_DFU);
    //ble_dfu_buttonless_bootloader_start_finalize();

    err_code = err_code;
}
// for send hex data to peripheral
const unsigned char hex2val_tabL[] =
{
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x0A,0x0B,0x0C,0x0D,0x0E,0x0F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x0A,0x0B,0x0C,0x0D,0x0E,0x0F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
};
static uint32_t nus_c_hexstring_sendout(const uint8_t * p_hexString, uint16_t conn_handle)
{
    static uint8_t nus_sendout_ary[NRF_SDH_BLE_GATT_MAX_MTU_SIZE];
    uint16_t length;
    uint32_t ret_val = NRF_ERROR_INVALID_PARAM;
    for (length=0; p_hexString[0] && p_hexString[1]; p_hexString+=2)
    {
        nus_sendout_ary[length++] = (hex2val_tabL[p_hexString[0]]<<4)+hex2val_tabL[p_hexString[1]];
        if(length >= sizeof(nus_sendout_ary)/sizeof(nus_sendout_ary[0])) break;
    }
    if (length>0)
    {
        //nus_sendout_ary[length] = 0; NRF_LOG_INFO("%s",nus_sendout_ary);
        do
        {
            ret_val = ble_nus_c_string_send(&m_nus_c[conn_handle], nus_sendout_ary, length);
            if ( (ret_val != NRF_ERROR_INVALID_STATE) && (ret_val != NRF_ERROR_BUSY) )
            {
                APP_ERROR_CHECK(ret_val);
            }
        } while (ret_val == NRF_ERROR_BUSY);
    }
    return ret_val;
}
static uint16_t nus_c_getHandleByPeerAddrString(const char * pszAddr)
{
    uint16_t conn_handle;
    char sz_peer_addr[BLE_GAP_ADDR_LEN*3+1];
    for (conn_handle=NRF_SDH_BLE_CENTRAL_LINK_COUNT-1; conn_handle>0; conn_handle--)
    {
        if (m_nus_c[conn_handle].conn_handle == BLE_CONN_HANDLE_INVALID) continue;
        sprintf(sz_peer_addr, "%02X-%02X-%02X-%02X-%02X-%02X", 
                    m_nus_c[conn_handle].peer_addr.addr[0],
                    m_nus_c[conn_handle].peer_addr.addr[1],
                    m_nus_c[conn_handle].peer_addr.addr[2],
                    m_nus_c[conn_handle].peer_addr.addr[3],
                    m_nus_c[conn_handle].peer_addr.addr[4],
                    m_nus_c[conn_handle].peer_addr.addr[5] );
        if (strcasecmp(sz_peer_addr, pszAddr) == 0)
        {
            break;
        }
    }
    return conn_handle;
}

static void host_command_main(int32_t argc, char * argv[])
{
    uint32_t err_code;
    if (argc>0)
    {
        NRF_LOG_DEBUG("%s",argv[0]);
        
        #if 1
        if (strcmp(argv[0],"#gpio_read") == 0) // uint32_t pin_number:0~31, return uint32_t status:0/1
        {
            if (argc>1)
            {
                uint32_t pin_number;
                sscanf(argv[1], "%d", &pin_number);
                err_code = nrf_gpio_pin_read(pin_number);
                printf("%s ok %d\r\n", argv[0], err_code);
            }
        }
        else
        if (strcmp(argv[0],"#gpio_set") == 0) // uint32_t pin_number:0~31
        {
            if (argc>1)
            {
                uint32_t pin_number;
                sscanf(argv[1], "%d", &pin_number);
                nrf_gpio_pin_set(pin_number);
                printf("%s ok\r\n", argv[0]);
            }
        }
        else
        if (strcmp(argv[0],"#gpio_clear") == 0) // uint32_t pin_number:0~31
        {
            if (argc>1)
            {
                uint32_t pin_number;
                sscanf(argv[1], "%d", &pin_number);
                nrf_gpio_pin_clear(pin_number);
                printf("%s ok\r\n", argv[0]);
            }
        }
        else
        if (strcmp(argv[0],"#gpio_toggle") == 0) // uint32_t pin_number:0~31
        {
            if (argc>1)
            {
                uint32_t pin_number;
                sscanf(argv[1], "%d", &pin_number);
                nrf_gpio_pin_toggle(pin_number);
                printf("%s ok\r\n", argv[0]);
            }
        }
        else
        if (strcmp(argv[0],"#gpio_cfg_output") == 0) // uint32_t pin_number:0~31
        {
            if (argc>1)
            {
                uint32_t pin_number;
                sscanf(argv[1], "%d", &pin_number);
                nrf_gpio_cfg_output(pin_number);
                printf("%s ok\r\n", argv[0]);
            }
        }
        else
        if (strcmp(argv[0],"#gpio_cfg_input") == 0) // uint32_t pin_number:0~31, nrf_gpio_pin_pull_t pull_config:0~2
        {
            if (argc>2)
            {
                uint32_t pin_number;
                uint32_t pull_config;
                sscanf(argv[1], "%d", &pin_number);
                sscanf(argv[2], "%d", &pull_config);
                nrf_gpio_cfg_input(pin_number, (nrf_gpio_pin_pull_t)pull_config);
                printf("%s ok\r\n", argv[0]);
            }
        }
        else
        if (strcmp(argv[0],"#ble_send") == 0) // char * HexString(FIFO), return uint32_t err_code: 0/others
        {
            uint16_t conn_handle = (argc>2)?nus_c_getHandleByPeerAddrString(argv[2]):0;
            err_code = (argc>1)?nus_c_hexstring_sendout((const uint8_t *)argv[1], conn_handle):NRF_SUCCESS;
            printf("%s ok %d\r\n", argv[0], err_code);
        }
        else
        if (strcmp(argv[0],"#ble_conn") == 0) // int32_t conn_handle:-1~0, return int32_t conn_handle:-1~0, 48bit BT addr: 12 HexString(LSB)
        {
            int32_t conn_handle = 0;
            if (argc>1)
            {
                sscanf(argv[1], "%i", &conn_handle);
            }
            if (conn_handle<0)
            {
                err_code = 0;
                for (conn_handle=0; conn_handle<NRF_SDH_BLE_CENTRAL_LINK_COUNT; conn_handle++)
                {
                    err_code += (m_nus_c[conn_handle].conn_handle == BLE_CONN_HANDLE_INVALID)?0:(1<<conn_handle);
                }
                printf("%s ok %d %d\r\n", argv[0], -1, err_code);
            }
            else
            if (m_nus_c[conn_handle].conn_handle == BLE_CONN_HANDLE_INVALID)
            {
                printf("%s ok %d\r\n", argv[0], m_nus_c[conn_handle].conn_handle);
            }
            else
            {
                printf("%s ok %d ", argv[0], m_nus_c[conn_handle].conn_handle);
                printf("%02X-%02X-%02X-%02X-%02X-%02X\r\n", 
                    m_nus_c[conn_handle].peer_addr.addr[0],
                    m_nus_c[conn_handle].peer_addr.addr[1],
                    m_nus_c[conn_handle].peer_addr.addr[2],
                    m_nus_c[conn_handle].peer_addr.addr[3],
                    m_nus_c[conn_handle].peer_addr.addr[4],
                    m_nus_c[conn_handle].peer_addr.addr[5] );
            }
        }
        else
        if (strcmp(argv[0],"#ble_close") == 0) // int32_t conn_handle:0~7, return ok/fail
        {
            uint32_t conn_handle = 0;
            if (argc>1)
            {
                sscanf(argv[1], "%i", &conn_handle);
            }
            if (conn_handle<NRF_SDH_BLE_CENTRAL_LINK_COUNT)
            {
                err_code = NRF_SUCCESS;
                if(m_nus_c[conn_handle].conn_handle != BLE_CONN_HANDLE_INVALID) 
                {
                    err_code = sd_ble_gap_disconnect(m_nus_c[conn_handle].conn_handle,
                                                     BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
                    if (err_code != NRF_ERROR_INVALID_STATE)
                    {
                        APP_ERROR_CHECK(err_code);
                    }
                }
                printf("%s ok %d\r\n", argv[0], err_code);
            }
            else printf("%s fail\r\n", argv[0]);
        }
        else
        #endif
        if (strcmp(argv[0],"#open") == 0)
        {
            host_do_open();
            printf("%s ok\r\n", argv[0]);
        }
        else
        if (strcmp(argv[0],"#close") == 0)
        {
            host_do_close();
            printf("%s ok\r\n", argv[0]);
        }
        else
        if (strcmp(argv[0],"#getinfo") == 0)
        {
            ble_gap_addr_t ble_addr;
            err_code = sd_ble_gap_addr_get(&ble_addr);
            if (err_code == NRF_SUCCESS)
            {
                printf("%s ok ", argv[0]);
                printf("%02X-%02X-%02X-%02X-%02X-%02X",  
                    ble_addr.addr[0],ble_addr.addr[1],ble_addr.addr[2],
                    ble_addr.addr[3],ble_addr.addr[4],ble_addr.addr[5]);
                printf(" %s %s\r\n", SW_MAIN_VERSION_S, __TIME__" "__DATE__);
            }
            else {printf("%s fail\r\n", argv[0]);}
        }
        else
        if (strcmp(argv[0],"#reset") == 0)
        {
            host_do_reset();
        }
        else
        if (strcmp(argv[0],"#dfu") == 0)
        {
            //printf("%s ok\r\n", sz_cmd);
            host_do_dfu();
        }
    }
}

static void host_command_do(uint8_t inbyte, uint8_t notreset)
{
    static char sz_cmd[16+NRF_SDH_BLE_GATT_MAX_MTU_SIZE*2] = {0};
    uint8_t  cmd_len;

    if (notreset == 0) sz_cmd[0] = '\0';
    if (inbyte == '\r' ||
        inbyte == '\n')
    {
        if (sz_cmd[0] != '\0')
        {
            char *    argv[16] = {0};
            int32_t argc = 0;
            
            argc = JL_strToken2Ary(sz_cmd, " \t", argv, sizeof(argv)/sizeof(argv[0]));

            host_command_main(argc, argv);

            sz_cmd[0] = '\0';
        }
    }
    else
    {
        cmd_len = strlen(sz_cmd);
        if (cmd_len < sizeof(sz_cmd)/sizeof(sz_cmd[0])-1)
        {
            sz_cmd[cmd_len] = inbyte;
            sz_cmd[cmd_len+1] = '\0';
        }
    }
    
}

#if 0
static void host_sendout_do(uint8_t inbyte, uint8_t notreset)
{
    static uint8_t data_array[NRF_SDH_BLE_GATT_MAX_MTU_SIZE];
    static uint16_t index = 0;

    if (notreset == 0) {index = 0;}
    data_array[index++] = inbyte;
    
    if ((data_array[index - 1] == '\n') || (index >= (m_ble_nus_max_data_len)))
    {
        NRF_LOG_DEBUG("Ready to send data over BLE NUS");
        NRF_LOG_HEXDUMP_DEBUG(data_array, index);

        nus_c_string_sendout(data_array, index);

        index = 0;
    }
}
#endif
/**
 * @}
 */
void user_task_command_recv(void * p_event_data, uint16_t event_size)
{
    uint8_t data;
    uint8_t * pu8event_cnt;
    if (p_event_data && (event_size==sizeof(pu8event_cnt)))
    {
        memcpy(&pu8event_cnt, p_event_data, sizeof(pu8event_cnt));
        *pu8event_cnt = 0;
    }
    while (app_uart_get(&data) == NRF_SUCCESS)
    {
        #if 0
        static uint8_t pair_mod = 0;
        static uint8_t uart_dir_mod=0xFF;
        switch (data)
        {
        case 0xFE: // to ble uart
        case 0xFF: // command
            uart_dir_mod = data;
            pair_mod     = 0;
            break;
        default:
            {
                switch (uart_dir_mod)
                {
                case 0xFE: // to ble uart
                    host_sendout_do(data, pair_mod);
                    pair_mod = 1;
                    break;
                case 0xFF: // command
                    host_command_do(data, pair_mod);
                    pair_mod = 1;
                    break;
                default:
                    break;
                }
            }
            break;
        }
        #else
        host_command_do(data, 1);
        #endif
    }

}


